#include <opencv2/opencv.hpp>
#include <iostream>

using namespace cv;
using namespace std;


int essentialmat_ex()
{
	int i, point_count = 35;
	vector<Point2f> rpoints(point_count);
	vector<Point2f> lpoints(point_count);

	Mat Ml(3, 3, CV_64F);
	Mat Mr(3, 3, CV_64F);

	FileStorage fs("cam_l.yml", FileStorage::READ);
	if (!fs.isOpened())
		return -1;
	fs["camera_matrix"] >> Ml;
	fs.release();

	fs.open("cam_l.yml", FileStorage::READ);
	if (!fs.isOpened())
		return -1;
	fs["camera_matrix"] >> Mr;
	fs.release();

	cout << "Mr:" << endl << Mr << endl;
	cout << "Ml:" << endl << Ml << endl;

	Mat rim = imread("lf_1.jpg", IMREAD_GRAYSCALE);
	Mat lim = imread("lf_4.jpg", IMREAD_GRAYSCALE);
	findChessboardCorners(rim, Size(5, 7), rpoints,
		CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
	findChessboardCorners(lim, Size(5, 7), lpoints,
		CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
	/*	cout << "right:" << endl;
	for (i=0;i<point_count;i++)
	cout << rpoints[i] << endl;
	cout << "left:" << endl;
	for (i=0;i<point_count;i++)
	cout << lpoints[i] << endl;*/
	cornerSubPix(rim, rpoints, Size(5, 7),
		Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));
	cornerSubPix(lim, lpoints, Size(5, 7),
		Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));

	for (i = 0; i<point_count; i++)
	{
		rpoints[i].x = (rpoints[i].x - *((double*)Mr.data + 2)) / *((double*)Mr.data);
		rpoints[i].y = (rpoints[i].y - *((double*)Mr.data + 5)) / *((double*)Mr.data + 4);
		lpoints[i].x = (lpoints[i].x - *((double*)Ml.data + 2)) / *((double*)Ml.data);
		lpoints[i].y = (lpoints[i].y - *((double*)Ml.data + 5)) / *((double*)Ml.data + 4);
		//		cout << "r:" << rpoints[i] << endl;
		//		cout << "l:" << lpoints[i] << endl;
	}

	Mat E_mat = findEssentialMat(rpoints, lpoints, 1, Point2d(0, 0), RANSAC, 0.999, 1.f);
	cout << "E:" << endl << E_mat << endl;

	/*	Mat rr(3,1,CV_64F);
	Mat ll(1,3,CV_64F);
	*((double*)rr.data+2)=1;
	*((double*)ll.data+2)=1;
	for (i=0;i<8;i++){
	*((double*)rr.data) = rpoints[i].x;
	*((double*)rr.data+1) = rpoints[i].y;
	*((double*)ll.data) = lpoints[i].x;
	*((double*)ll.data+1) = lpoints[i].y;
	cout << ll*E_mat*rr << endl;
	cout << rr.t()*E_mat*ll.t() << endl;
	}
	Mat R1, R2, t;
	decomposeEssentialMat(E_mat, R1, R2, t);
	cout << "R1:" << endl << R1 << endl;
	cout << "R2:" << endl << R2 << endl;
	cout << "t:" << endl << t << endl;*/

	Mat R, t;
	recoverPose(E_mat, rpoints, lpoints, R, t);
	cout << "t:" << endl << t << endl;
	cout << "R:" << endl << R << endl;
	Mat om;
	Rodrigues(R, om);
	cout << "om:" << endl << om << endl;


	return 0;
}